
#include "digital_LPF.h"

/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void SLAVG_f_init(STR_SLAVG_f *hdl, float32_t *temp, uint16_t length)
{
	hdl->value = 0;
	hdl->sum = 0;
	hdl->cnt = 0;
	
	hdl->length = length;
	hdl->temp = temp;
}



/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

float32_t SLAVG_f(STR_SLAVG_f *hdl, float32_t value)
{
    float32_t result = 0;
	
	hdl->value = (float32_t)(value);

    hdl->sum = hdl->sum - hdl->temp[hdl->cnt] + hdl->value;

    *(hdl->temp + hdl->cnt) = hdl->value;

    hdl->cnt++;
    hdl->cnt %= hdl->length;

    result = hdl->sum / (float32_t)hdl->length;
	
	hdl->rslt = result;

    return hdl->rslt;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void LPF_1ST_order_init(STR_1ST_LPF *hdl, float32_t fs, float32_t fc)
{
	float32_t a = 0;
	
	a = 2 * PI * fc / fs;
	
	hdl->sf = a;
	hdl->value_last = 0;
}


/*!
    \brief      
    \param[in]  none
    \param[out] none
    \retval     none
*/

void LPF_2ND_order_init(STR_2ND_LPF *hdl, float32_t fs, float32_t fc)
{
	float32_t wm = 0;
	float32_t c = 0;
	float32_t A1, A2, B0, B1, B2;
	float32_t a1, a2, b0, b1, b2;
	
	wm = tanf(PI * fc / fs);
	
	c = 1 + 1.414f * wm + wm * wm;
	
	B0 = wm * wm;
	B1 = 2 * B0;
	B2 = B0;
	
	A1 = 2 * wm *wm - 2;
	A2 = 1 - 1.414f * wm + wm * wm;
	
	b0 = B0 / c;
	b1 = B1 / c;
	b2 = B2 / c;
	
	a1 = A1 / c;
	a2 = A2 / c;
	
	hdl->b0 = b0;
	hdl->b1 = b1;
	hdl->b2 = b2;
	
	hdl->a1 = a1;
	hdl->a2 = a2;
}

